#include "stm32f10x.h"                  // Device header
#include "myi2c.h"
#include "MPU6050_REG.H"
#include "math.h"

#define MPU6050_Address 0xD0


void MPU6050_writeReg(unsigned char regaddress,unsigned data)
{
	Myi2c_start();
	Myi2c_send_byte(MPU6050_Address);
	Myi2c_receive_ack();
	Myi2c_send_byte(regaddress);
	Myi2c_receive_ack();
	Myi2c_send_byte(data);
	Myi2c_receive_ack();
	Myi2c_stop();
}

unsigned char MPU6050_readReg(unsigned char regaddress)
{
	unsigned char data;
	Myi2c_start();
	Myi2c_send_byte(MPU6050_Address);
	Myi2c_receive_ack();
	Myi2c_send_byte(regaddress);
	Myi2c_receive_ack();
	
	Myi2c_start();
	Myi2c_send_byte(MPU6050_Address | 0x01);
	Myi2c_receive_ack();
	data=Myi2c_receive_byte();
	Myi2c_send_ack(1);
	Myi2c_stop();
	return data;
}

void MPU6050_init(void)
{
	Myi2c_init();
	MPU6050_writeReg(MPU6050_PWR_MGMT_1,0X01);
	MPU6050_writeReg(MPU6050_PWR_MGMT_2,0X00);
	MPU6050_writeReg(MPU6050_SMPLRT_DIV,0X09);
	MPU6050_writeReg(MPU6050_CONFIG,0X06);
	MPU6050_writeReg(MPU6050_GYRO_CONFIG,0X18);
	MPU6050_writeReg(MPU6050_ACCEL_CONFIG,0X18);
}

unsigned char MPU6050_getID(void)
{
	return MPU6050_readReg(MPU6050_WHO_AM_I);
}

void MPU6050_getData(short int *accx,short int *accy,short int *accz,
										 short int *gyrox,short int *gyroy,short int *gyroz)
{
	unsigned char dataH,dataL;
	dataH=MPU6050_readReg(MPU6050_ACCEL_XOUT_H);
	dataL=MPU6050_readReg(MPU6050_ACCEL_XOUT_L);
	*accx=(dataH<<8)|dataL;
	
	dataH=MPU6050_readReg(MPU6050_ACCEL_YOUT_H);
	dataL=MPU6050_readReg(MPU6050_ACCEL_YOUT_L);
	*accy=(dataH<<8)|dataL;
	
	dataH=MPU6050_readReg(MPU6050_ACCEL_ZOUT_H);
	dataL=MPU6050_readReg(MPU6050_ACCEL_ZOUT_L);
	*accz=(dataH<<8)|dataL;
	
	dataH=MPU6050_readReg(MPU6050_GYRO_XOUT_H);
	dataL=MPU6050_readReg(MPU6050_GYRO_XOUT_L);
	*gyrox=(dataH<<8)|dataL;
	
	dataH=MPU6050_readReg(MPU6050_GYRO_YOUT_H);
	dataL=MPU6050_readReg(MPU6050_GYRO_YOUT_L);
	*gyroy=(dataH<<8)|dataL;
	
	dataH=MPU6050_readReg(MPU6050_GYRO_ZOUT_H);
	dataL=MPU6050_readReg(MPU6050_GYRO_ZOUT_L);
	*gyroz=(dataH<<8)|dataL;	
	
}

float MPU6050_Transform(float value,float URV,float LRV)
{
	float data;
	float value_L=-32767.0;
	float value_U=32767.0;
	data=(value-value_L)/(value_U-value_L)*(URV-LRV)+LRV;
	return data;
}

float MPU6050_temp_Transform(float temp)
{
	return temp/340+36.53;
}


